#include <iostream>
// For disable PCL complile lib, to use PointXYZIR
#define PCL_NO_PRECOMPILE

#include <ros/ros.h>
#include <std_msgs/String.h>
#include <signal.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>

#include "patchworkpp/patchworkpp.hpp"
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/filters/radius_outlier_removal.h>


using PointType = PointXYZILID;
using namespace std;

boost::shared_ptr<PatchWorkpp<PointType>> PatchworkppGroundSeg;

ros::Publisher pub_cloud;
ros::Publisher pub_ground;
ros::Publisher pub_non_ground;

std::string cloud_src_;
template<typename T>
sensor_msgs::PointCloud2 cloud2msg(pcl::PointCloud<T> cloud, const ros::Time& stamp, std::string frame_id = "map") {
    sensor_msgs::PointCloud2 cloud_ROS;
    pcl::toROSMsg(cloud, cloud_ROS);
    cloud_ROS.header.stamp = stamp;
    cloud_ROS.header.frame_id = frame_id;
    return cloud_ROS;
}

// void crop_box_filter(const pcl::PointCloud<PointType>::Ptr in,
//                                    const pcl::PointCloud<PointType>::Ptr out)
// {
//     pcl::ExtractIndices<PointType> cliper;

//     cliper.setInputCloud(in);
//     pcl::PointIndices indices;
// #pragma omp for
//     for (size_t i = 0; i < in->points.size(); i++)
//     {
//         // if (in->points[i].z > 2.5 || in->points[i].z < -0.4 ||
//         if(
//             in->points[i].x > 50 || in->points[i].x < -30 ||
//             in->points[i].y > 30 || in->points[i].y < -30)
//         {
//             indices.indices.push_back(i);
//         }
//     }
//     cliper.setIndices(boost::make_shared<pcl::PointIndices>(indices));
//     cliper.setNegative(true); //ture to remove the indices
//     cliper.filter(*out);
// }
void callbackCloudSrc(const std_msgs::String::Ptr &cloud_msg)
{
    cloud_src_ = cloud_msg->data;
}

void callbackCloud(const sensor_msgs::PointCloud2::Ptr &cloud_msg)
{
    if(cloud_src_ != "new_src")
        return;
    double time_taken;

    pcl::PointCloud<PointType> pc_curr;
    pcl::PointCloud<PointType> pc_ground;
    pcl::PointCloud<PointType> pc_non_ground;

    pcl::fromROSMsg(*cloud_msg, pc_curr);

    PatchworkppGroundSeg->estimate_ground(pc_curr, pc_ground, pc_non_ground, time_taken);

    ROS_INFO_STREAM("\033[1;32m" << "Input PointCloud: " << pc_curr.size() << " -> Ground: " << pc_ground.size() <<  "/ NonGround: " << pc_non_ground.size()
         << " (running_time: " << time_taken << " sec)" << "\033[0m");

    pub_ground.publish(cloud2msg(pc_ground, cloud_msg->header.stamp, cloud_msg->header.frame_id));
    pub_non_ground.publish(cloud2msg(pc_non_ground, cloud_msg->header.stamp, cloud_msg->header.frame_id));

}

int main(int argc, char**argv) {

    ros::init(argc, argv, "Demo");
    ros::NodeHandle nh;
    ros::NodeHandle pnh("~");

    std::string input_topic, nonground_topic, ground_topic;
    pnh.param<string>("input_topic", input_topic, "/input");
    pnh.param<string>("nonground_topic", nonground_topic, "/nonground");
    pnh.param<string>("ground_topic", ground_topic, "/ground");

    cout << "Operating patchwork++..." << endl;
    cloud_src_ = "new_src";
    PatchworkppGroundSeg.reset(new PatchWorkpp<PointType>(&pnh));

    // pub_cloud       = pnh.advertise<sensor_msgs::PointCloud2>("cloud", 100, true);
    pub_ground      = pnh.advertise<sensor_msgs::PointCloud2>(ground_topic, 100, true);
    pub_non_ground  = pnh.advertise<sensor_msgs::PointCloud2>(nonground_topic, 100, true);

    //debug
    

    ros::Subscriber sub_cloud = nh.subscribe(input_topic, 10, callbackCloud);

    ros::Subscriber sub_cloud_src = nh.subscribe("/cloud_src", 1, callbackCloudSrc);
        
    ros::spin();

    return 0;
}
